clear;
close all;
dbstop if error
%% 添加casadi
addpath("E:\Coding\matlab\casadi-3.6.7-windows64-matlab2018b");
addpath("base_func\");
import casadi.*

%% 处理线路数据
% 线路1
% CurveData = readmatrix("real_line\北京北-崇礼\曲线文件输入-递增.xls");
% GradientData = readmatrix("virtual_line\line_info.xlsx","Sheet","坡度2");
% SpeedLimitData = readmatrix("virtual_line\line_info.xlsx","Sheet","限速3");

% 线路2
GradientData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","坡道");
CurveData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","曲线");
CurveData = [CurveData(:,1:2).*1000 CurveData(:,4)];
SpeedLimitData = readmatrix("real_line\cz再造线路-v9-评审后修改版本.xlsx","Sheet","限速");

%处理成仅3列，且把 km->m
GradientData = [GradientData(:,1:2).*1000 GradientData(:,3)];
CurveData = [CurveData(:,1:2).*1000 CurveData(:,3)];
SpeedLimitData = [SpeedLimitData(:,1:2).*1000 SpeedLimitData(:,3)];

%曲线数据补充平直道
CurveData = fill_curvedata(CurveData);
%补充缺失的限速数据
SpeedLimitData = fill_speedlimit(SpeedLimitData,250);

%% 结构体1：线路参数
LineInfo.gradient = GradientData;
LineInfo.curve = CurveData;
LineInfo.speedlimit = SpeedLimitData;
LineInfo.init_height = 640; %初始海拔

%% 结构体2：列车参数
TrainInfo.abc = [0.55 0.003622 0.00011]; %CRH380B
TrainInfo.mass = 425; %吨 CRH380B - AW1
TrainInfo.rf = 0.06; % rotate factor 回转质量系数
TrainInfo.eta_gear = 0.9; %齿轮箱输出和输入功率之比
TrainInfo.motor_num = 16;

%% 结构体3：求解参数
QsInfo.pos_start = 400;   %起点
QsInfo.pos_end = 550; %终点
QsInfo.v_start = 0; % km/h
QsInfo.v_end = 0;
QsInfo.set_time = 3500; %计划剩余时间
QsInfo.t_now = 0; %起始时刻
QsInfo.step_x = 20; %求解步长1
QsInfo.step_t = 5; 
QsInfo.t_err = 5; %达时运行浮动时间
QsInfo.T_st = [90 98]; %起始温度

%% 计算
%计算最大能力曲线
% state = speedPlanMP(LineInfo, TrainInfo, QsInfo);
% state = speedPlanMP(LineInfo, TrainInfo, QsInfo);
% t_mp_min = state(end,3);
%计算恒速曲线
state = speedplan_cruise(LineInfo, TrainInfo, QsInfo);
% xvtfhg = [state(:,1:3) state(:,4)-state(:,5) state(:,13) state(:,11)];
tvxfhg_temp = convert_pos_to_time_discrete(state, QsInfo);

%牵引计算恒速优化
Tcr_(1,:) = QsInfo.T_st;
vcr_kmh = zeros(length(state),1);
tcr = zeros(length(state),1);
fcr_ = zeros(length(state),1);
ploss_cr = zeros(length(state),4);
mode = 0;
%位置离散牵引计算
for i = 1:length(state)-1
    % 牵引力
    fcr_(i) = state(i,4) - state(i,5);

    % 计算基本运行阻力
    f_rb = cal_unit_basicResist(TrainInfo.abc,vcr_kmh(i)) * TrainInfo.mass * 9.81 / 1000;
    % 计算线路附加阻力
    f_rl = cal_unit_lineResist(state(i,1),LineInfo.gradient,LineInfo.curve) * TrainInfo.mass * 9.81 / 1000;
    % 计算加速度
    a = (fcr_(i)-f_rb-f_rl)/TrainInfo.mass/(1+TrainInfo.rf);
    % 计算下一状态速度
    vcr_kmh(i+1) = sqrt(vcr_kmh(i)*vcr_kmh(i)/3.6^2 + 2*a*QsInfo.step_x) * 3.6;
    % 更新过程时间
    dt_ = 2*QsInfo.step_x/((vcr_kmh(i+1) + vcr_kmh(i))/3.6);
    tcr(i+1) = tcr(i) + dt_;

    v_kmh = (state(i,2)+state(i+1,2))/2;
    % v_kmh = state(i,2);
    if fcr_(i) < -getMaxFeb(v_kmh)
        Fpmt = -getMaxFeb(v_kmh)/TrainInfo.motor_num;
    else
        Fpmt = fcr_(i)/TrainInfo.motor_num;
    end

    [ploss_cr(i,1),ploss_cr(i,2),ploss_cr(i,3),ploss_cr(i,4)] = getmotorlossv2(Fpmt, v_kmh);
    dt_ = tcr(i+1)-tcr(i);
    [Tcr_(i+1,1), Tcr_(i+1,2)] = lptn2nodes_sr(ploss_cr(i,1:2),Tcr_(i,1:2),state(i,13),dt_);
end
%% 初始优化
%权重 速度 | 位置 | 温度 | 电牵制力代价 | 空气制动代价 | 终端代价
w1 = 1e-1;
w2 = 1e-2;
w3 = 50;
w4 = 1e-2;
w5 = 400;
w6 = 1e6;
weights = [w1 w2 w3 w4 w5 w6];
[res, casadi_flag] = atc_opt_v4(tvxfhg_temp, weights, LineInfo, TrainInfo, QsInfo, []);
last_res = res; %对于下一次优化而言存储上一次优化结果
% res = [td_' res];
%% 创建动态图窗
figure(3)
yyaxis right;
hl1 = area(state(:,1)./1000,state(:,13),'FaceColor',[0.7 0.7 0.7],'EdgeColor', [0.6 0.6 0.6],"FaceAlpha",0.35);
set(hl1, 'DisplayName', '坡度');
ylim([0 5000]);
xlim([QsInfo.pos_start-10 QsInfo.pos_end+10]);
yyaxis left;
h1 = plot(res(:,3)./1000,res(:,2).*3.6,'LineWidth',1.5);
set(h1, 'DisplayName', "原始优化结果");
hold on;
xlabel('位置km');
ylabel('速度km/h');
% draw_res(res,3);
%% 执行nmpc过程
tst_nmpc = tic;
t_end = QsInfo.set_time + QsInfo.t_err; %计算终止时间
qs_info = QsInfo; %用于事件触发重优化的结构体
dt = 1; %牵引计算时间步长
tc = [0:dt:t_end]';
sc = zeros(length(tc),1);
f_ = zeros(length(tc),1);
fe_ = zeros(length(tc),1);
fm_ = zeros(length(tc),1);
sc(1) = QsInfo.pos_start*1000;
vc = zeros(length(tc),1); % m/s
ploss = zeros(length(tc),4);
T_ = zeros(length(tc),2);
T_(1,:) = QsInfo.T_st;
last_mod = 0;
flag_opt = 0; %事件触发标志
pc_count = 1;
opt_proc = {};
tk = 0; %记录重规划次数
for i=1:length(tc)
    t = tc(i);
    for j = 1:length(res)-1
        if t >= res(j,1) && t <= res(j+1,1) %寻找优化结果中的对应序列
            v_p = res(j,2); %预测的速度
            s_p = res(j,3); %预测的位置 
            Ts_p = res(j,4); %预测的定子温度
            Tr_p = res(j,5); %预测的转子温度
            fe_(i) = res(j,6); %电机发挥力
            fm_(i) = res(j,7); %空气制动力
            f_(i) = fe_(i) - fm_(i);
            Fpmt = fe_(i) / TrainInfo.motor_num; %单个电机的发挥力
            break;
        end
        if t>res(end,1)  %超过预设时间，应该制动了
            v_p = 0; %预测的速度
            s_p = qs_info.pos_end; %预测的位置 
            Ts_p = res(j,4); %预测的定子温度
            Tr_p = res(j,5); %预测的转子温度
            fe_(i) = res(end,6); %电机发挥力
            fm_(i) = 200; %空气制动力
            f_(i) = fe_(i) - fm_(i);
            Fpmt = fe_(i) / TrainInfo.motor_num; %单个电机的发挥力
            break;
        end
    end
    % 牵引计算
    % 计算基本运行阻力
    f_rb = cal_unit_basicResist(TrainInfo.abc,vc(i)*3.6) * TrainInfo.mass * 9.81 / 1000;
    % 计算线路附加阻力
    f_rl = cal_unit_lineResist(sc(i),LineInfo.gradient,LineInfo.curve) * TrainInfo.mass * 9.81 / 1000;
    % 计算加速度
    a = (f_(i)-f_rb-f_rl)/TrainInfo.mass/(1+TrainInfo.rf);
    % 计算下一状态速度
    vc(i+1) = vc(i) + a*dt;
    % 计算下一状态位置
    sc(i+1) = sc(i) + vc(i)*dt + 1/2*a*dt^2;
    % 计算下一状态时间
    tn = t + dt;
    % 计算电机损耗
    [ploss(i,1),ploss(i,2),ploss(i,3),ploss(i,4)] = getmotorlossv2(Fpmt, vc(i+1)*3.6);
    % 解算电机实际温度
    h = getPosHeight(sc(i+1),LineInfo);
    [T_(i+1,1), T_(i+1,2)] = lptn2nodes_sr(ploss(i,1:2),T_(i,1:2),h,dt);
    % 判断制动停车
    if vc(i+1) < 0
        vc(i+1) = 0;
    end
    % 事件触发重规划
    % xvtfhg_temp = xvtfhg;
    switch flag_opt
        case 0 %状态0：判断事件触发重规划
            t_s0 = t; %记录状态0时间戳

            % 事件触发重规划：周期重规划
            % tk = floor(t/240);
            % if abs(res(j,3)-sc(i+1)) > 50 && tk > last_mod && QsInfo.set_time-t >= 120
            %     flag_opt = 1;
            %     last_mod = tk;
            % end
            % 速度偏离事件
            if abs(vc(i)-v_p) > 1.1
                flag_opt = 1;
            end
            % 温度偏离事件
            if T_(i,1)-Ts_p > 10 || T_(i,2)-Tr_p > 10
                flag_opt = 1;
            end
            % 剩余时间过少事件
            if QsInfo.set_time-t < 20
                flag_opt = 0;
            end
        case 1 %重规划
            tst_plan = tic;
            tk = tk+1;
            %重规划更新求解配置
            qs_info.pos_start = sc(i+1)./1000;
            qs_info.v_start = vc(i+1)*3.6;
            qs_info.t_now = tn; %更新当前时间
            qs_info.set_time = QsInfo.set_time - qs_info.t_now;  %更新剩余时间
            qs_info.T_st = [T_(i+1,1), T_(i+1,2)];
            %根据运行时间调整权重
            w2 = 1e-2 + (1-1e-2)*t/QsInfo.set_time;
            weights = [w1 w2 w3 w4 w5 w6];
            %调整时间步长
            if qs_info.set_time/qs_info.step_t < 200 && qs_info.step_t > 1
                qs_info.step_t = qs_info.step_t - 1;
            end
            
            %更新恒速曲线
            t_cruise_st = tic;
            state_temp = speedplan_cruise(LineInfo, TrainInfo, qs_info);
            tvxfhg_temp = convert_pos_to_time_discrete(state_temp,qs_info);
            fprintf("-函数恒速规划耗时：%f s\n",toc(t_cruise_st));
 
            %优化求解
            t_optf_st = tic;
            [res, casadi_flag] = atc_opt_v4(tvxfhg_temp, weights, LineInfo, TrainInfo, qs_info, last_res);
            fprintf("-函数atc_opt耗时：%f s\n",toc(t_optf_st));
            opt_proc{1,tk} = res;
            %判断是否优化成功
            if ~casadi_flag
                for io = tk:-1:1
                    if ~isempty(opt_proc{1,io})
                        res = opt_proc{1,io};
                        last_res = res;
                        break;
                    end
                end
            end
            % res = [td_' res];
            % h1 = plot(res(:,3)./1000,res(:,2).*3.6);
            % set(h1, 'DisplayName', "第"+num2str(tk)+"次");
            plot(qs_info.pos_start,qs_info.v_start,'Marker','|','Color','g','LineWidth',3,'HandleVisibility', 'off');
            % h2 = plot(sc(pc_count:i+1)./1000,vc(pc_count:i+1)*3.6,'LineWidth',2,'LineStyle',':','Color',[0.6350 0.0780 0.1840],'HandleVisibility', 'off');
            % pc_count = i+2;
            lgd = legend('show','Location','south');
            % draw_res(res,4);
            flag_opt = 2; %跳到状态2
            ted_plan = toc(tst_plan);
            fprintf("--第"+num2str(tk)+"次滚动优化"+"耗时：%f s\n",ted_plan);
        case 2 %重规划最小间隔
            if t - t_s0 > 30 
                flag_opt = 0;
            end
    end
end
%衔接上面的figure3
h2 = plot(sc./1000,vc*3.6,'LineWidth',2,'LineStyle',':','Color',[0.6350 0.0780 0.1840],'HandleVisibility', 'on', 'Marker','none');
set(h2, 'DisplayName', "牵引计算");
ylim([0 250]);
hold off;
ted_nmpc = toc(tst_nmpc);
fprintf("总计用时：%f s\n",ted_nmpc);

%% 效果统计
T_stator = real(T_(:,1));
T_rotor = real(T_(:,2));
%位置误差统计
x_end_err = qs_info.pos_end*1000 - sc(end);
fprintf("终点误差：%f m\n",x_end_err);
% 查找停车时刻
i = length(vc);
t_stop = tc(i-1);
while vc(i) == 0
    t_stop = tc(i-1);
    i = i-1;
end
t_end_err = t_stop - QsInfo.set_time;
fprintf("时间误差：%f s\n",t_end_err);
%能耗统计：优化
recycle_ratio = 0.5;
energy_opt = 0;
energy_opt_brake = 0;
for i=1:length(fe_)
    dx_ = sc(i+1) - sc(i);
    energy_opt = energy_opt + fe_(i)*dx_*(fe_(i)>0); %纯牵引能耗
end
energy_opt_kwh = energy_opt / 3600;
%能耗统计：恒速
energy_cruise = state(1:end-1,4)' * (state(2:end,1)-state(1:end-1,1)); %纯牵引能耗
energy_cruise_brake = state(1:end-1,5)' * (state(2:end,1)-state(1:end-1,1)); %电制动能耗
energy_cruise_kwh = energy_cruise / 3600;
fprintf("轮周牵引能耗—— 恒速：%f kWh | nmpc：%f kWh\n",energy_cruise_kwh,energy_opt_kwh);
%能耗统计：耗散
motor_loss_cruise_kwh = TrainInfo.motor_num .* real(ploss_cr(1:end-1,3))' * real(tcr(2:end)-tcr(1:end-1))  ./ 3600; %恒速耗散
motor_loss_nmpc_kmh = TrainInfo.motor_num .* real(ploss(1:end-1,3))' * real(tc(2:end)-tc(1:end-1))  ./ 3600;
%牵引节能率
ee_ratio = (energy_cruise_kwh-energy_opt_kwh)/energy_cruise_kwh * 100;
fprintf("牵引能耗节能率：%f\n",ee_ratio);
%电机输入功率侧节能率
pmotor_in_cruise = energy_cruise_kwh+motor_loss_cruise_kwh;
pmotor_in_nmpc = energy_opt_kwh+motor_loss_nmpc_kmh;
ee_ratio2 = (pmotor_in_cruise - pmotor_in_nmpc)/pmotor_in_cruise * 100;
fprintf("电机侧输入能耗—— 恒速：%f kWh | nmpc：%f kWh\n",pmotor_in_cruise,pmotor_in_nmpc);
fprintf("电机输入能耗节能率：%f\n",ee_ratio2);
%再生制动总有效功率 回馈制动能量减去发热耗散的能量
%恒速优化下的再生制动总能耗
fec_ = zeros(length(state)-1,1);
for i=1:length(state)-1
    if state(i,5) > 0
        v_temp = state(i,2);
        fec_(i,1) = min([state(i,5) getMaxFeb(v_temp)]); %恒速优化下的制动力包括了电制动力和空气制动力
    end
end
ec1_vec = fec_ .* (state(2:end,1)-state(1:end-1,1)) - ...
        TrainInfo.motor_num .*ploss_cr(1:end-1,3).*(tcr(2:end)-tcr(1:end-1)).*(state(1:end-1,5)>0);
ec1 = sum(ec1_vec)./3600;
%nmpc优化下的再生制动总能耗
eo1_vec = -fe_(1:end-1).*(sc(2:end-1) - sc(1:end-2)).*(fe_(1:end-1)<0) - ...
    TrainInfo.motor_num .*ploss(1:end-1,3).*(tc(2:end)-tc(1:end-1)).*(fe_(1:end-1)<0);
eo1 = sum(eo1_vec)./3600;
fprintf("电机侧再生制动能量—— 恒速：%f kWh | nmpc：%f kWh\n",ec1,eo1);
%考虑再生制动节能率
energy_cruise_plus_eb = pmotor_in_cruise-ec1*recycle_ratio;
energy_opt_plus_eb = pmotor_in_nmpc-eo1*recycle_ratio;
ee_ratio3 = (energy_cruise_plus_eb-energy_opt_plus_eb) / energy_cruise_plus_eb * 100;
fprintf("能耗考虑再生制动—— 恒速：%f kWh | nmpc：%f kWh\n",energy_cruise_plus_eb, energy_opt_plus_eb);
fprintf("考虑%f再生制动利用的节能率：%f\n",recycle_ratio, ee_ratio3);

%计算温度主动抑制效果
cr_Ts_avg = sum(Tcr_(:,1))/length(Tcr_(:,1)); %恒速-定子-平均温度
cr_Ts_max = max(Tcr_(:,1));
cr_Tr_avg = sum(Tcr_(:,2))/length(Tcr_(:,2)); %恒速-转子-平均温度
cr_Tr_max = max(Tcr_(:,2));
opt_Ts_avg = sum(T_stator)/length(T_stator); %nmpc-定子-平均温度
opt_Ts_max = max(T_stator);
opt_Tr_avg = sum(T_rotor)/length(T_rotor); %nmpc-转子-平均温度
opt_Tr_max = max(T_rotor);
fprintf("定子——平均温度降低：%f | 最高温度降低：%f\n", cr_Ts_avg-opt_Ts_avg, cr_Ts_max-opt_Ts_max);
fprintf("转子——平均温度降低：%f | 最高温度降低：%f\n", cr_Tr_avg-opt_Tr_avg, cr_Tr_max-opt_Tr_max);

%% 画图
%值采样
x_km = sc./1000;
v_kmh = vc*3.6;
t_s = tc;
f_kN = [f_; f_(end)];

%计算坡度
init_h = LineInfo.init_height;
h_(1) = init_h;
for j = 1:length(LineInfo.gradient)
    h_(j+1) = h_(j) + (LineInfo.gradient(j,2)-LineInfo.gradient(j,1))*LineInfo.gradient(j,3)/1000;
end
h_ = h_';
%处理限速
start_points = LineInfo.speedlimit(:, 1);
end_points = LineInfo.speedlimit(:, 2);
speed_values = LineInfo.speedlimit(:, 3);
vl_x = [];
vl_y = [];
for i2 = 1:size(LineInfo.speedlimit, 1)
    % 每段限速数据
    vl_x = [vl_x; start_points(i2); end_points(i2)];
    vl_y = [vl_y; speed_values(i2); speed_values(i2)];
end
vl_x = vl_x./1000; %m->km
%判断上下行
if QsInfo.pos_start > QsInfo.pos_end
    upordown = -1;
else
    upordown = 1;
end

figure(1)
set(gcf,'Position',[100,50,800,900]);
[ha,pos] = tight_subplot(4, 1,[.005 .05],[.05 .03],[.08 .07]); % 缩小子图间距
axes(ha(1));
hl3 = plot(state(:,1)./1000,vcr_kmh,'LineWidth',2,'Color',[0.8500 0.3250 0.0980]);
hold on;
set(hl3, 'DisplayName', "恒速优化")
hl2 = plot(x_km,v_kmh,'LineWidth',2,'Color',[0 0.4470 0.7410]);
set(hl2, 'DisplayName', "实际速度")
% hl3 = plot(new_mat(:,2),new_mat(:,8),'LineStyle',':','LineWidth',2.5,'Color',[0.8500 0.3250 0.0980]);
% set(hl3, 'DisplayName', "参考速度")
hl4 = plot(vl_x,vl_y,'LineStyle',':','LineWidth',1.5,'Color',	[1 0 1]);
set(hl4, 'DisplayName', "限速")
% xlim([min(new_mat(:,2))-0.2 max(new_mat(:,2))+0.2]);
% ylim([0 max(v_kmh)+20]);
xlabel("位置（km）");
ylabel("速度（km/h）");
if upordown == -1
    set(gca, 'XDir', 'reverse');
end
lgd = legend('show','Location','south');
axis([min(x_km)-0.2 max(x_km)+0.2 0 max(LineInfo.speedlimit(:,3))+20]);
% set(gca, 'XTickMode', 'auto', 'XTickLabelMode', 'auto');
% grid on;

% set(lgd, 'Position', [0, 0, 50, 60]); % 自定义位置
% title("空间域跟踪效果图");
axes(ha(2));
hl1 = area([LineInfo.gradient(:,1);LineInfo.gradient(end,2)]./1000,h_,'FaceColor',[0.7 0.7 0.7],'EdgeColor', [0.6 0.6 0.6],"FaceAlpha",0.35);
set(hl1, 'DisplayName', '坡度');
grid on;
legend("坡度")
xlim([min(x_km)-0.2 max(x_km)+0.2]);
ylim([min(h_)-50 max(h_)*1.3]);
ylabel("相对海拔（m）");
if upordown == -1
    set(gca, 'XDir', 'reverse');
end

axes(ha(3));
plot(state(:,1)./1000,fcr_,'Color',[0.8500 0.3250 0.0980]);
hold on;
plot(x_km,f_kN,'LineWidth',2,'Color',[0 0.4470 0.7410]);
plot(x_km,[fe_;fe_(end)],'LineStyle','--');
plot(x_km,-[fm_;fm_(end)],'LineStyle','-.');
% plot(new_mat(:,2),new_mat(:,5),'LineStyle',':','LineWidth',2.5);
% plot(new_mat(:,2),new_mat(:,7),'LineStyle','--','LineWidth',1.5);
plot([-1 1e6],[0 0],'LineStyle','--','Color',[0 0 0])
xlabel("位置（km）");
ylabel("力（kN）");
axis([min(x_km)-0.2 max(x_km)+0.2 min(f_kN)-50 max(f_kN)+50]);
if upordown == -1
    set(gca, 'XDir', 'reverse');
end
% grid on;
legend("恒速优化力","实际力","电机力","空气制动力","Location","south");

axes(ha(4))
plot(x_km,T_stator,'LineWidth',2);
hold on;
plot(x_km,T_rotor,'LineWidth',2);
plot(state(:,1)./1000,Tcr_(:,1),'LineStyle','--');
plot(state(:,1)./1000,Tcr_(:,2),'LineStyle','--');
grid on;
axis([min(x_km)-0.2 max(x_km)+0.2 min(T_rotor)-20 200]);
xlabel("位置（km）");
ylabel("温度（℃）");
legend("定子温度","转子温度","恒速定子温度","恒速转子温度","Location","northwest");
%% 画时域图
figure(2)
set(gcf,'Position',[100,50,800,700]);
[ha,pos] = tight_subplot(3, 1,[.005 .05],[.07 .03],[.08 .07]); % 缩小子图间距
axes(ha(1));
plot(state(:,3),state(:,2),'LineWidth',2,'Color',[0.8500 0.3250 0.0980]);
hold on;
plot(tc,v_kmh(1:end-1),'LineWidth',2,'Color',[0 0.4470 0.7410]);
ylabel("速度（km/h）");

axes(ha(2))
plot(tc,fe_,'LineWidth',2,'Color',[0 0.4470 0.7410]);
hold on;
plot([0 1e6],[0 0],'LineStyle','--','Color',[0 0 0])
% xlim([0 QsInfo.set_time]);
ylabel("力（kN）");

axes(ha(3))
plot(tc,T_stator(1:end-1),'LineWidth',2)
hold on;
plot(tc,T_rotor(1:end-1),'LineWidth',2);
plot(state(:,3),Tcr_(:,1),'LineStyle','--');
plot(state(:,3),Tcr_(:,2),'LineStyle','--');
grid on;
xlabel("时间（s）");
ylabel("温度（℃）");
legend("定子温度","转子温度","恒速定子温度","恒速转子温度","Location","northwest");
%% 画每次优化的温度结果
figure(77)
subplot(2,1,1)
for i = 1:numel(opt_proc)
    proc_res = opt_proc{1,i};
    if isempty(proc_res)
        continue;
    end
    h2 = plot(proc_res(:,1),proc_res(:,5));
    hold on;
    plot(proc_res(1,1),proc_res(1,5),'Marker','|','Color','r','LineWidth',3,'HandleVisibility', 'off');
    set(h2, 'DisplayName', "第"+num2str(i)+"次优化温度线");
end
h2 = plot(tc,T_rotor(1:end-1),'LineWidth',2,'LineStyle','--');
set(h2, 'DisplayName', "实际温度增长线");
lgd = legend('show','Location','northwest');
xlabel("时间 s");
ylabel("转子温度 ")

subplot(2,1,2)
for i = 1:numel(opt_proc)
    proc_res = opt_proc{1,i};
    if isempty(proc_res)
        continue;
    end
    h2 = plot(proc_res(:,1),proc_res(:,4));
    hold on;
    plot(proc_res(1,1),proc_res(1,4),'Marker','|','Color','r','LineWidth',3,'HandleVisibility', 'off');
    set(h2, 'DisplayName', "第"+num2str(i)+"次优化温度线");
end
h2 = plot(tc,T_stator(1:end-1),'LineWidth',2,'LineStyle','--');
set(h2, 'DisplayName', "实际温度增长线");
lgd = legend('show','Location','northwest');
xlabel("时间 s");
ylabel("定子温度 ");
